/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-11-03 14:39:20
 * @LastEditTime: 2021-11-21 00:19:05
 * @LastEditors: Sang Hao
 */
#ifndef LIDAR_SLAM_MODELS_SEGMENTATION_MESH_DBSCAN_SEG_HPP_
#define LIDAR_SLAM_MODELS_SEGMENTATION_MESH_DBSCAN_SEG_HPP_

#include <vector>
#include "lidar_slam/sensor_data/mesh_data.hpp"
#include "lidar_slam/models/meshing/polar_meshing.hpp"
#include "lidar_slam/models/segmentation/cloud_segment_interface.hpp"

namespace lidar_slam {
class MeshDbscanSegment : public CloudSegmentInterface {
private:
	/* data */
	/* 地面分割三参数 */
	float threshold_angle_ = 10;
	float threshold_z_ = 0.1;
	float threshold_d_ = 0.03;

	/* 非地面分割参数 */
	int min_sample_;
	int radius_man_;

	int threshold_pointnum_ = 100;

	/* mesh参数 */
	std::shared_ptr<PolarMeshing> polar_meshing_ptr_;
	
private:
	bool SetSegParam(int min_sample, int radius_man);
	float Dist3(const CloudData::CLOUD_PTR& input_cloud_ptr, int id1, int id2);
	float Theta(const CloudData::CLOUD_PTR& input_cloud_ptr, int id1, int id2);

private:

	/* 分割的实现全程处理all_mesh_data */
	bool GroundSeg(const CloudData::CLOUD_PTR& input_cloud_ptr, std::map<int, MeshData>& all_mesh_data);

	// bool DBSCAN(std::vector<vector<int>>& data, std::vector<int>& label, int min_sample, int radius_man);

	bool NoGroundSeg(const CloudData::CLOUD_PTR& input_cloud_ptr, std::map<int, MeshData>& all_mesh_data);
	void GetOccuSet(std::map<int, MeshData>& all_mesh_data, std::set<int>& occu_set);
	int FindNeighbor(int query, std::set<int>& occu_set, std::set<int>& neighbor);
	void ClusterOne(int query, std::set<int>& occu_set, std::set<int>& temp, std::set<int>& clusterk);
	void DbscanSeg(std::map<int, MeshData>& all_mesh_data);

public:
	MeshDbscanSegment(const YAML::Node& config_node);
	MeshDbscanSegment(int min_sample, int radius_man, float res_distance, float res_angle);
	/* 保留的对外接口，对外接口不显含all_mesh_data，只保留类标签label */
	bool Segmentation(const CloudData::CLOUD_PTR& input_cloud_ptr,
		CloudRGBData::CLOUD_PTR& seged_cloud_ptr) override;
	
};
}
#endif